/*
 * PlayerMotionController.cpp
 *
 *  Created on: Jun 30, 2010
 *      Author: Ry
 */

#include <btBulletDynamicsCommon.h>
#include <iostream>
#include "math/Vector3.h"
#include "PlayerMotionController.h"

using namespace std;

PlayerMotionController::PlayerMotionController(Object *player) {
	this->player = player;
	player->setMotionController(this);

	// Set some defaults
	sensitivity = 5.0f;
	constantResist = 5.0;
	proportionalResist = 0.1f;

	maxVelocity = 5;

	pitch = 0.0f;
	yaw = 0.0f;
	moveForward = false;
	moveBackward = false;
	strafeLeft = false;
	strafeRight = false;
}

void PlayerMotionController::handleRotation(float dts) {
	// Grab the player's current transformation
	btTransform t = player->getTransform();

	Vector3 mouse = Vector3(pitch, yaw, 0) * sensitivity * dts;

	// Next, figure out rotation
	btQuaternion r = t.getRotation();
	r = r * btQuaternion(btVector3(1, 0, 0), btRadians(mouse.x)); // Pitch locally
	r = btQuaternion(btVector3(0, 1, 0), btRadians(mouse.y)) * r; // Yaw globally
	t.setRotation(r);

	// Add a proportional resist element
	Vector3 resist = -Vector3(pitch, yaw, 0) * proportionalResist * dts;

	// Add a constant resist element
	if (resist.length() > 0) {
		resist = resist + resist.normalize() * constantResist * dts;
	}

	// If the magnitude of the resistance is less than the magnitude of the
	// movement, then resistance can be safely added to the movement without
	// accidentally sening the movement in the opposite direction
	if (resist.length() < mouse.length()) {
		pitch = pitch + resist.x;
		yaw = yaw + resist.y;
	}

	// Otherwise, do not add resistance to movement, which would send the
	// movement in the opposite direction.  Simply zero out the movement.
	else {
		pitch = 0;
		yaw = 0;
	}

	player->setTransform(t);
}

void PlayerMotionController::handleMovement(float dts) {
	btTransform t = player->getTransform();
	btQuaternion r = t.getRotation();

	// Next, compute the player's new position
	btVector3 v(0, 0, 0);
	btVector3 p = t.getOrigin();

	btMatrix3x3 m(r.inverse()); // Create matrix from current rotation
	btVector3 forward = m.getRow(2); // +Z axis
	btVector3 left = m.getRow(0);    // +X axis
	btVector3 backward = -forward;   // -Z axis
	btVector3 right = -left;         // -X axis

	if (moveForward) {
		v = v + forward;
	}
	if (moveBackward) {
		v = v + backward;
	}
	if (strafeLeft) {
		v = v + left;
	}
	if (strafeRight) {
		v = v + right;
	}

	if (v.length() > 0) {
		v.normalize();
	}

	v = v * maxVelocity;
	t.setOrigin(p + (v * dts));
	player->setTransform(t);
}

void PlayerMotionController::step(float dts) {
	handleRotation(dts);
	handleMovement(dts);
}

PlayerMotionController::~PlayerMotionController() {
}
